#!/usr/bin/env python3
"""
机械臂控制节点-双位置模式版
功能：
- 模式1：关节角度控制（6个参数：5关节+夹爪）
- 模式2：双位置控制（6个参数：抓取点XYZ + 释放点XYZ）
"""

import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
import serial
import struct
from threading import Thread, Lock
import time
import binascii

class RobotArms(Node):
    def __init__(self):
        super().__init__('robot_arms_control')
        
        # --- 参数配置 ---
        self.declare_parameter('serial_port', '/dev/ttyUSB0')
        self.declare_parameter('baudrate', 115200)
        self.declare_parameter('bytesize', 8)      # 数据位 (7或8)
        self.declare_parameter('parity', 'N')      # 校验位 (N/E/O)
        self.declare_parameter('stopbits', 1)      # 停止位 (1/2)
        
        # --- 串口初始化(10ms超时) ---
        serial_port = self.get_parameter('serial_port').value
        baudrate = self.get_parameter('baudrate').value
        
        # 串口参数映射
        parity_map = {'N': serial.PARITY_NONE, 
                    'E': serial.PARITY_EVEN,
                    'O': serial.PARITY_ODD}
        
        try:
            self.serial_port = serial.Serial(
                port=serial_port,
                baudrate=baudrate,
                bytesize=self.get_parameter('bytesize').value,
                parity=parity_map[self.get_parameter('parity').value],
                stopbits=self.get_parameter('stopbits').value,
                timeout=1.0  # 10ms超时
            )
            
            # 打印完整的串口配置
            self.get_logger().info("\n" + "="*94)
            self.get_logger().info("串口初始化成功！配置参数：")
            self.get_logger().info(f"端口: {self.serial_port.port}")
            self.get_logger().info(f"波特率: {self.serial_port.baudrate}")
            self.get_logger().info(f"数据位: {self.serial_port.bytesize}")
            self.get_logger().info(f"校验位: {self.serial_port.parity}")
            self.get_logger().info(f"停止位: {self.serial_port.stopbits}")
            self.get_logger().info(f"超时: {self.serial_port.timeout}秒")
            self.get_logger().info("="*50 + "\n")
            
        except serial.SerialException as e:
            self.get_logger().error(f"串口初始化失败: {str(e)}")
            raise
        except KeyError:
            self.get_logger().error("无效的校验位配置，可选值: N(无校验), E(偶校验), O(奇校验)")
            raise
        
        self.serial_lock = Lock()
        
        # --- 双控制模式 ---
        # 角度指令（6个参数）
        self.angle_sub = self.create_subscription(
            Float64MultiArray,
            'arm_angle_commands',
            self.angle_callback,
            10
        )
        
        # 双位置指令（6个参数）
        self.position_sub = self.create_subscription(
            Float64MultiArray,
            'arm_position_commands',
            self.position_callback,
            10
        )

        self.get_logger().info("节点已启动")
        self.get_logger().info("指令模式说明:")
        self.get_logger().info("1. 关节角度: /arm_angle_commands [ j1 , j2 ,  j3 , j4 , j5 , gripper ]")
        self.get_logger().info("2. 双位置点: /arm_position_commands [ grip_x , grip_y , grip_z , release_x , release_y , release_z ]")

         # ---------- 常驻接收线程 ----------
        self.recv_thread = Thread(target=self._serial_receive_loop, daemon=True)
        self.recv_thread.start()

    # def _receive_once_after_send(self, expect_header=0x5A):
    #     """发送后等待接收数据（优化版）"""
    #     HEADER = expect_header.to_bytes(1, 'little')
    #     DATA_SIZE = 6 * 8  # 6个double
    #     PACKET_SIZE = 1 + DATA_SIZE
        
    #     def _receive_worker():
    #         buffer = bytearray()
    #         start_time = time.time()
    #         self.get_logger().info(f"开始等待下位机回传数据(超时:6s)... 预期包头:0x{expect_header:02X}")
            
    #         while (time.time() - start_time) < 6.0 and rclpy.ok():
    #             try:
    #                 # 非阻塞读取
    #                 with self.serial_lock:
    #                     data = self.serial_port.read_all()
                    
    #                 if data:
    #                     self.get_logger().debug(f"收到原始数据({len(data)}字节): {binascii.hexlify(data).decode()}")
    #                     buffer.extend(data)
                        
    #                     # 持续检查缓冲区
    #                     while len(buffer) >= PACKET_SIZE:
    #                         header_pos = buffer.find(HEADER)
                            
    #                         if header_pos < 0:
    #                             buffer.clear()
    #                             self.get_logger().debug("未找到包头，清空缓冲区")
    #                             break
                                
    #                         if header_pos > 0:
    #                             self.get_logger().debug(f"跳过{header_pos}字节前导数据")
    #                             buffer = buffer[header_pos:]
                            
    #                         if len(buffer) < PACKET_SIZE:
    #                             break
                            
    #                         # 提取完整数据包
    #                         pkt = buffer[:PACKET_SIZE]
    #                         buffer = buffer[PACKET_SIZE:]
                            
    #                         try:
    #                             angles = struct.unpack('<6d', pkt[1:])
    #                             self.get_logger().info(
    #                                 "收到完整回传:\n"
    #                                 f"J1={angles[0]:.3f} J2={angles[1]:.3f} J3={angles[2]:.3f}\n"
    #                                 f"J4={angles[3]:.3f} J5={angles[4]:.3f} Gripper={angles[5]:.3f}"
    #                             )
    #                             return  # 成功接收后退出
                                
    #                         except struct.error as e:
    #                             self.get_logger().warn(f"数据包解析失败: {str(e)}")
    #                             continue
                    
    #                 time.sleep(0.01)  # 短时间等待
                    
    #             except serial.SerialException as e:
    #                 self.get_logger().error(f"串口读取失败: {str(e)}")
    #                 break
    #             except Exception as e:
    #                 self.get_logger().error(f"接收线程异常: {type(e).__name__}: {str(e)}")
    #                 break
            
    #         self.get_logger().warn(f"等待回传超时! 已接收{len(buffer)}字节数据: {binascii.hexlify(buffer).decode()}")
    #         if buffer:
    #             self.get_logger().warn("缓冲区残留数据(可能不完整):")
    #             self.get_logger().warn(f"Hex: {binascii.hexlify(buffer).decode()}")
    #             self.get_logger().warn(f"ASCII: {buffer.decode('ascii', errors='replace')}")

    #     Thread(target=_receive_worker, daemon=True).start()

        # ------------------------------------------------------------------
    # 常驻串口接收线程
    # ------------------------------------------------------------------
    def _serial_receive_loop(self):
        """
        持续监听串口，解析 0x5A + 6 double 帧（49 B）
        解析成功后直接打印，不做任何 ROS 阻塞调用
        """
        HEADER = b'\x5A'
        PACKET_SIZE = 1 + 6 * 8  # 49 字节
        buffer = bytearray()

        while rclpy.ok():
            try:
                # 非阻塞读
                with self.serial_lock:
                    data = self.serial_port.read(self.serial_port.in_waiting or 1)
                if not data:
                    time.sleep(0.01)
                    continue

                buffer.extend(data)

                # 循环处理完整帧
                while len(buffer) >= PACKET_SIZE:
                    idx = buffer.find(HEADER)
                    if idx == -1:
                        buffer.clear()          # 没找到帧头，全部丢弃
                        break
                    if idx > 0:
                        buffer = buffer[idx:]   # 去掉帧头前的垃圾
                    if len(buffer) < PACKET_SIZE:
                        break                   # 数据还不够

                    # 拿到完整包
                    packet = buffer[:PACKET_SIZE]
                    buffer = buffer[PACKET_SIZE:]

                    try:
                        angles = struct.unpack('<6d', packet[1:])
                        self.get_logger().info(
                            f"收到角度回传："
                            f"J1={angles[0]:.2f}  "
                            f"J2={angles[1]:.2f}  "
                            f"J3={angles[2]:.2f}  "
                            f"J4={angles[3]:.2f}  "
                            f"J5={angles[4]:.2f}  "
                            f"Gripper={angles[5]:.2f}"
                        )
                    except struct.error:
                        self.get_logger().warn("解析帧数据失败，丢弃")
            except serial.SerialException as e:
                self.get_logger().error(f"串口异常：{e}")
                time.sleep(1)
            except Exception as e:
                self.get_logger().error(f"接收线程异常：{type(e).__name__}: {e}")
                time.sleep(0.5)
                               
    def angle_callback(self, msg):
        """ 关节角度指令 (6个参数) """
        if len(msg.data) == 6:
            self._send_serial(header=0x5A, data=msg.data)
            # self._receive_once_after_send(expect_header=0x5A)
            self.get_logger().info(
                f"角度指令已发送: J1={msg.data[0]:.2f} J2={msg.data[1]:.2f} J3={msg.data[2]:.2f} J4={msg.data[3]:.2f} J5={msg.data[4]:.2f} Gripper={msg.data[5]:.2f}",
                throttle_duration_sec=1.0
            )
        else:
            self.get_logger().error("需要6个参数[5关节+夹爪]")

    def position_callback(self, msg):
        """ 双位置点指令 (6个参数) """
        if len(msg.data) == 6:
            self._send_serial(header=0xA5, data=msg.data)
            # self._receive_once_after_send(expect_header=0x5A)
            self.get_logger().info(
                "位置指令已发送:\n"
                f"抓取点: ({msg.data[0]:.2f}, {msg.data[1]:.2f}, {msg.data[2]:.2f})\n"
                f"释放点: ({msg.data[3]:.2f}, {msg.data[4]:.2f}, {msg.data[5]:.2f})",
                throttle_duration_sec=1.0
            )
        else:
            self.get_logger().error("需要6个坐标参数[grip_xyz, release_xyz]")

    def _send_serial(self, header, data):
        """串口数据发送核心方法（线程安全）"""
        with self.serial_lock:
            try:
                # 构造数据包：帧头 + 6个double
                packet = bytearray([header])  # 帧头
                for value in data:
                    packet.extend(struct.pack('<d', value))  # 小端double
                
                # 发送数据
                bytes_written = self.serial_port.write(packet)
                self.serial_port.flush()
                
                # 调试信息
                self.get_logger().debug(
                    f"发送 {bytes_written} 字节: "
                    f"帧头=0x{header:02X} 数据={data}",
                    throttle_duration_sec=1.0)
                    
                return True
                
            except serial.SerialException as e:
                self.get_logger().error(f"串口发送失败: {str(e)}")
                return False
            except struct.error as e:
                self.get_logger().error(f"数据打包失败: {str(e)}")
                return False
            except Exception as e:
                self.get_logger().error(f"未知发送错误: {type(e).__name__}: {str(e)}")
                return False

    def destroy_node(self):
        """ 清理资源 """
        with self.serial_lock:
            if self.serial_port.is_open:
                self.serial_port.close()
        super().destroy_node()
        self.get_logger().warn(f"手动关闭节点！！！！")

def main(args=None):
    rclpy.init(args=args)
    node = RobotArms()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()